#include "mavlink_handler.h"
#include <inttypes.h>  // 用于PRIu32宏
static const char *TAG = "MAVLINK";


uint8_t target_system = 255;
uint8_t target_component = 1;
// 全局状态变量
mavlink_state_t mavlink_state = {
    .self_check_passed = false,
    .is_armed = false,
    .has_target = false,
    .custom_mode = 0,
    .mission_state = MISSION_STATE_IDLE,
    .mission_count = 0,
    .mission_current_index = 0,
    .arm_command_timer = 0,
    .arm_command_sent = false,
    .guide_send_timer = 0,
    .guide_sent = false,
    .prearm_status = {
        .system_standby = false,
        .not_armed = false,
        .no_prearm_errors = true,
        .ekf_ok = false,
        .gps_ok = false,
        .battery_ok = true,
        .last_check_time = 0
    }
};

// 互斥锁
SemaphoreHandle_t mavlink_state_mutex = NULL;

// 初始化MAVLink状态保护
void mavlink_state_init(void) {
    if (mavlink_state_mutex == NULL) {
        mavlink_state_mutex = xSemaphoreCreateMutex();
        if (mavlink_state_mutex == NULL) {
            ESP_LOGE(TAG, "创建MAVLink状态互斥锁失败!");
        } else {
            ESP_LOGI(TAG, "MAVLink状态保护机制初始化成功");
        }
    }
}

// 锁定状态（带超时）
bool mavlink_state_lock(TickType_t timeout) {
    if (mavlink_state_mutex == NULL) {
        ESP_LOGE(TAG, "互斥锁未初始化!");
        return false;
    }
    
    if (xSemaphoreTake(mavlink_state_mutex, timeout) == pdTRUE) {
        return true;
    } else {
        ESP_LOGW(TAG, "获取MAVLink状态锁超时!");
        return false;
    }
}

// 释放状态锁
void mavlink_state_unlock(void) {
    if (mavlink_state_mutex != NULL) {
        xSemaphoreGive(mavlink_state_mutex);
    }
}

// ===============================================================================
// 安全的状态读取函数
bool mavlink_get_has_target(void) {
    bool has_target = false;
    if (MAVLINK_STATE_LOCK()) {
        has_target = mavlink_state.has_target;
        MAVLINK_STATE_UNLOCK();
    }
    return has_target;
}


bool mavlink_get_armed_status(void) {
    bool status = false;
    if (MAVLINK_STATE_LOCK()) {
        status = mavlink_state.is_armed;
        MAVLINK_STATE_UNLOCK();
    }
    return status;
}

bool mavlink_get_self_check_status(void) {
    bool status = false;
    if (MAVLINK_STATE_LOCK()) {
        status = mavlink_state.self_check_passed;
        MAVLINK_STATE_UNLOCK();
    }
    return status;
}

mission_state_t mavlink_get_mission_state(void) {
    mission_state_t state = MISSION_STATE_IDLE;
    if (MAVLINK_STATE_LOCK()) {
        state = mavlink_state.mission_state;
        MAVLINK_STATE_UNLOCK();
    }
    return state;
}

uint16_t mavlink_get_mission_count(void) {
    uint16_t count = 0;
    if (MAVLINK_STATE_LOCK()) {
        count = mavlink_state.mission_count;
        MAVLINK_STATE_UNLOCK();
    }
    return count;
}

uint16_t mavlink_get_mission_index(void) {
    uint16_t index = 0;
    if (MAVLINK_STATE_LOCK()) {
        index = mavlink_state.mission_current_index;
        MAVLINK_STATE_UNLOCK();
    }
    return index;
}

// ===============================================================================
// 安全的状态设置函数

void mavlink_set_armed_status(bool armed) {
    if (MAVLINK_STATE_LOCK()) {
        mavlink_state.is_armed = armed;
        MAVLINK_STATE_UNLOCK();
        ESP_LOGI(TAG, "设置解锁状态: %s", armed ? "已解锁" : "已上锁");
    }
}

void mavlink_set_self_check_status(bool passed) {
    if (MAVLINK_STATE_LOCK()) {
        mavlink_state.self_check_passed = passed;
        MAVLINK_STATE_UNLOCK();
        ESP_LOGI(TAG, "设置自检状态: %s", passed ? "通过" : "失败");
    }
}

void mavlink_set_has_target(bool has) {
    if (MAVLINK_STATE_LOCK()) {
        mavlink_state.has_target = has;
        MAVLINK_STATE_UNLOCK();
        ESP_LOGI(TAG, "设置装订目标: %s", has ? "已装订" : "未装订");
    }
}

void mavlink_set_mission_state(mission_state_t state) {
    if (MAVLINK_STATE_LOCK()) {
        mavlink_state.mission_state = state;
        MAVLINK_STATE_UNLOCK();
        
        const char* state_str[] = {"空闲", "清除中", "上传中", "完成"};
        ESP_LOGI(TAG, "设置航线状态: %s", state_str[state]);
    }
}

void mavlink_set_mission_count(uint16_t count) {
    if (MAVLINK_STATE_LOCK()) {
        mavlink_state.mission_count = count;
        MAVLINK_STATE_UNLOCK();
        ESP_LOGI(TAG, "设置航线数量: %d", count);
    }
}

void mavlink_set_mission_index(uint16_t index) {
    if (MAVLINK_STATE_LOCK()) {
        mavlink_state.mission_current_index = index;
        MAVLINK_STATE_UNLOCK();
        ESP_LOGI(TAG, "设置航线索引: %d", index);
    }
}

// 原子设置航线上传状态（避免多次加锁）
void mavlink_set_mission_upload_state(uint16_t count, uint16_t index, mission_state_t state) {
    if (MAVLINK_STATE_LOCK()) {
        mavlink_state.mission_count = count;
        mavlink_state.mission_current_index = index;
        mavlink_state.mission_state = state;
        MAVLINK_STATE_UNLOCK();
        
        ESP_LOGI(TAG, "设置航线上传状态: 数量=%d, 索引=%d, 状态=%d", count, index, state);
    }
}


static mavlink_message_t rx_msg;
static mavlink_status_t rx_status;

// 解锁重试间隔
// static const uint32_t ARM_RETRY_INTERVAL = 300; // 3秒重试一次 (300 * 10ms = 3000ms)

void mavlink_uart_init(void)
{
    uart_config_t uart_config = {
        .baud_rate = 115200,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_XTAL
    };

    ESP_ERROR_CHECK(uart_driver_install(MAVLINK_UART_NUM, MAVLINK_BUF_SIZE, MAVLINK_BUF_SIZE, 0, NULL, 0));
    ESP_ERROR_CHECK(uart_param_config(MAVLINK_UART_NUM, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(MAVLINK_UART_NUM, MAVLINK_TXD_PIN, MAVLINK_RXD_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
    
    ESP_LOGI(TAG, "MAVLink UART initialized on UART%d (TX:%d, RX:%d)", 
             MAVLINK_UART_NUM, MAVLINK_TXD_PIN, MAVLINK_RXD_PIN);
}

bool mavlink_check_prearm_ready(void)
{
    prearm_check_t *prearm = &mavlink_state.prearm_status;
    uint32_t current_time = xTaskGetTickCount() * portTICK_PERIOD_MS;

    // 检查预解锁错误超时重置逻辑
    if (!prearm->no_prearm_errors && prearm->last_prearm_error_time > 0) {
        uint32_t error_elapsed = current_time - prearm->last_prearm_error_time;
        const uint32_t ERROR_TIMEOUT_MS = 2000; // 2秒超时
        
        if (error_elapsed > ERROR_TIMEOUT_MS) {
            ESP_LOGI(TAG, "预解锁错误超时重置: %lu ms后未收到新错误，重置为true", error_elapsed);
            prearm->no_prearm_errors = true;
            prearm->last_prearm_error_time = 0; // 清除时间戳
        }
    }

    // 根据飞行模式决定是否需要GPS
    bool gps_required = false;
    const char* mode_name = "未知模式";
    
    switch (mavlink_state.custom_mode) {
        case 0:  // 自稳模式 (STABILIZE)
            gps_required = false;
            mode_name = "自稳模式";
            break;
        case 16: // 位置保持模式 (POSHOLD)
            gps_required = true;
            mode_name = "位置保持模式";
            break;
        case 18: // 引导模式 (GUIDED)
            gps_required = true;
            mode_name = "引导模式";
            break;
        default:
            gps_required = true;
            mode_name = "其他模式";
            break;
    }

    bool basic_ready = prearm->system_standby && 
                      prearm->not_armed && 
                      prearm->no_prearm_errors && 
                      prearm->ekf_ok && 
                      prearm->battery_ok;

    bool ready = basic_ready && (gps_required ? prearm->gps_ok : true);
    
    // 每5秒打印一次详细状态
    uint32_t current_time_t = xTaskGetTickCount() * portTICK_PERIOD_MS;
    if (current_time_t - prearm->last_check_time > 5000) {
        prearm->last_check_time = current_time_t;
        ESP_LOGI(TAG, "=== 准备解锁状态检查 ===");
        ESP_LOGI(TAG, "  飞行模式: %s (custom_mode=%lu)", mode_name, mavlink_state.custom_mode);
        ESP_LOGI(TAG, "  GPS要求: %s", gps_required ? "需要" : "不需要");
        ESP_LOGI(TAG, "  系统待机: %s", prearm->system_standby ? "✅ Yes" : "❌ No");
        ESP_LOGI(TAG, "  未解锁: %s", prearm->not_armed ? "✅ Yes" : "❌ No");
        ESP_LOGI(TAG, "  无预解锁错误: %s", prearm->no_prearm_errors ? "✅ Yes" : "❌ No");
        ESP_LOGI(TAG, "  EKF状态: %s", prearm->ekf_ok ? "✅ Yes" : "❌ No");
        
        // 添加错误超时状态显示
        if (!prearm->no_prearm_errors && prearm->last_prearm_error_time > 0) {
            uint32_t error_elapsed = current_time_t - prearm->last_prearm_error_time;
            ESP_LOGI(TAG, "  错误持续时间: %lu ms (将在 %lu ms后重置)", 
                    error_elapsed, 2000 - error_elapsed);
        }

        if (gps_required) {
            ESP_LOGI(TAG, "  GPS状态: %s", prearm->gps_ok ? "✅ Yes" : "❌ No");
        } else {
            ESP_LOGI(TAG, "  GPS状态: 🔄 当前模式不需要GPS");
        }
        
        ESP_LOGI(TAG, "  电池状态: %s", prearm->battery_ok ? "✅ Yes" : "❌ No");
        ESP_LOGI(TAG, "  总体状态: %s", ready ? "✅ 准备解锁" : "❌ 未准备解锁");
        ESP_LOGI(TAG, "========================");
    }
    
    return ready;
}

void mavlink_send_heartbeat(void)
{
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];

    mavlink_msg_heartbeat_pack(
        255,                      // 系统 ID
        1,                       // 组件 ID
        &msg,
        MAV_TYPE_GCS,           // 类型
        MAV_AUTOPILOT_GENERIC,  // 飞控类型
        MAV_MODE_MANUAL_ARMED,  // 模式
        0,                      // 自定义模式
        MAV_STATE_ACTIVE        // 状态
    );

    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(MAVLINK_UART_NUM, (const char*)buf, len);
}

void mavlink_send_arm_disarm_command(uint8_t target_system, uint8_t target_component, bool arm)
{
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];

    mavlink_msg_command_long_pack(
        1,                           // 系统 ID (发送方)
        1,                           // 组件 ID (发送方)
        &msg,
        target_system,               // 目标系统ID
        target_component,            // 目标组件ID
        MAV_CMD_COMPONENT_ARM_DISARM,// 命令ID: 解锁/上锁命令
        0,                           // 确认字段: 0表示首次发送
        arm ? 1.0f : 0.0f,          // param1: 1=解锁, 0=上锁
        0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
    );

    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(MAVLINK_UART_NUM, (const char*)buf, len);
    
    ESP_LOGI(TAG, "发送%s指令到系统%d组件%d", 
             arm ? "解锁" : "上锁", target_system, target_component);
}

void mavlink_send_guide_board_target(uint8_t target_sys, uint8_t target_comp,
                                     int32_t lat, int32_t lon, float alt, int8_t target_type)
{
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];

    mavlink_msg_guide_board_pack(
        target_sys, target_comp,
        &msg,
        lat, lon, alt, target_type
    );

    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(MAVLINK_UART_NUM, (const char *)buf, len);

    ESP_LOGI(TAG, "📡 已发送目标点 GUIDE_BOARD: lat=%ld lon=%ld alt=%.2f", lat, lon, alt);
}

int mavlink_parse_guide_ack_message(const mavlink_message_t* msg, guide_ack_info_t* ack_info)
{
    if (!msg || !ack_info) {
        ESP_LOGE(TAG, "错误: 输入参数为空");
        return -1;
    }

    if (msg->msgid != MAVLINK_MSG_ID_GUIDE_ACK) {
        ESP_LOGE(TAG, "错误: 消息ID不匹配，期望%d，实际%d", 
                MAVLINK_MSG_ID_GUIDE_ACK, msg->msgid);
        return -1;
    }

    mavlink_guide_ack_t guide_ack;
    mavlink_msg_guide_ack_decode(msg, &guide_ack);

    ack_info->upload_flag = guide_ack.upload_flag;
    
    if (guide_ack.upload_flag == GUIDE_ACK_SUCCESS) {
        ack_info->result = GUIDE_ACK_SUCCESS;
        ack_info->result_msg = "目标位置装订成功";
    } else {
        ack_info->result = GUIDE_ACK_FAILED;
        ack_info->result_msg = "目标位置装订失败";
    }

    return 0;
}

// 发送航点数量
void mavlink_send_mission_count(uint8_t target_system, uint8_t target_component, uint16_t count)
{
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];

    mavlink_msg_mission_count_pack(
        1,              // 系统 ID
        1,              // 组件 ID
        &msg,
        target_system,  // 目标系统ID
        target_component, // 目标组件ID
        count,          // 航线点数量
        0               // 任务类型
    );

    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(MAVLINK_UART_NUM, (const char*)buf, len);
    
    ESP_LOGI(TAG, "发送航线数量: %d", count);
}

void send_mission_items(uart_port_t uart_num, uint8_t target_system, uint8_t target_component, uint16_t requested_seq) {
    // 获取当前任务数据
    drone_task_t mission_data;
    if (!get_current_mission_data(&mission_data)) {
        ESP_LOGE(TAG, "无法获取航线数据");
        return;
    }

    // 验证请求的序号
    if (requested_seq >= mission_data.waypoint_count) {
        ESP_LOGE(TAG, "请求的航点序号超出范围: %d >= %d", requested_seq, mission_data.waypoint_count);
        return;
    }

    // 获取请求的航点数据
    waypoint_t *waypoint = &mission_data.waypoints[requested_seq];

    if (requested_seq == 0) {
        // 第1个点：无效点，发送一个简单的航点
        int32_t lat_int = (int32_t)(waypoint->lat * 1e7);
        int32_t lon_int = (int32_t)(waypoint->lon * 1e7);
        
        send_mission_item_int(uart_num, target_system, target_component,
            requested_seq, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT,
            0, 1, 0, 2.0f, 0, 0, lat_int, lon_int, waypoint->alt);
            
        ESP_LOGI(TAG, "发送无效点 %d", requested_seq);
        
    } else if (requested_seq == 1) {
        // 第2个点：速度设置点
        send_mission_item_int(uart_num, target_system, target_component,
            requested_seq, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED,
            0, 1, 
            1.0f,                    // param1: 速度类型 (1=地速)
            waypoint->speed,         // param2: 速度值 (m/s)
            -1.0f,                   // param3: 油门 (-1=不改变)
            0,                       // param4: 相对或绝对 (0=绝对)
            0, 0, 0);
            
        ESP_LOGI(TAG, "发送速度设置点 %d: %.2f m/s", requested_seq, waypoint->speed);
        
    } else if (requested_seq == 2) {
        // 第3个点：起飞点
        int32_t lat_int = (int32_t)(waypoint->lat * 1e7);
        int32_t lon_int = (int32_t)(waypoint->lon * 1e7);
        
        send_mission_item_int(uart_num, target_system, target_component,
            requested_seq, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_TAKEOFF,
            0, 1, 0, 0, 0, 0, lat_int, lon_int, waypoint->alt);
            
        ESP_LOGI(TAG, "发送起飞点 %d: 高度=%.2f", requested_seq, waypoint->alt);
        
    } else {
        // 第4个点及以后：正常航线点
        int32_t lat_int = (int32_t)(waypoint->lat * 1e7);
        int32_t lon_int = (int32_t)(waypoint->lon * 1e7);
        
        send_mission_item_int(uart_num, target_system, target_component,
            requested_seq, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT,
            0, 1, 0, 2.0f, 0, 0, lat_int, lon_int, waypoint->alt);
            
        ESP_LOGI(TAG, "发送航线点 %d: 位置=(%.7f,%.7f), 高度=%.2f", 
                requested_seq, waypoint->lat, waypoint->lon, waypoint->alt);
    }

    ESP_LOGI(TAG, "航点 %d/%d 发送完成", requested_seq, mission_data.waypoint_count);
}

void send_mission_item_int(uart_port_t uart_num, uint8_t target_system, uint8_t target_component, 
                          uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, 
                          uint8_t autocontinue, float param1, float param2, float param3, float param4,
                          int32_t x, int32_t y, float z)
{
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];

    mavlink_msg_mission_item_int_pack(
        1,              // 系统 ID
        1,              // 组件 ID
        &msg,
        target_system,  // 目标系统ID
        target_component, // 目标组件ID
        seq,            // 序列号
        frame,          // 坐标系
        command,        // 命令
        current,        // 是否为当前点
        autocontinue,   // 自动继续
        param1,         // 参数1
        param2,         // 参数2
        param3,         // 参数3
        param4,         // 参数4
        x,              // 纬度 * 10^7 或 本地坐标 * 1e4
        y,              // 经度 * 10^7 或 本地坐标 * 1e4
        z,              // 高度 (米)
        0               // 任务类型
    );

    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(uart_num, (const char*)buf, len);
    
    ESP_LOGI(TAG, "发送航线点 %d: 命令=%d, 坐标=(%ld, %ld, %.2f)", 
             seq, command, x, y, z);
}

// 发送清除航线命令
void send_mission_clear_all(uart_port_t uart_num, uint8_t target_system, uint8_t target_component)
{
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];

    mavlink_msg_mission_clear_all_pack(
        1,              // 系统 ID
        1,              // 组件 ID
        &msg,
        target_system,  // 目标系统ID
        target_component, // 目标组件ID
        0               // 任务类型 (0 = MAV_MISSION_TYPE_MISSION)
    );

    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(uart_num, (const char*)buf, len);
    
    ESP_LOGI(TAG, "发送清除航线命令");
}

// ===== 添加回调函数支持 =====

// 回调函数指针
static guide_ack_callback_t guide_ack_callback = NULL;
static mission_ack_callback_t mission_ack_callback = NULL;

// 设置回调函数
void mavlink_set_guide_ack_callback(guide_ack_callback_t callback) {
    guide_ack_callback = callback;
}

void mavlink_set_mission_ack_callback(mission_ack_callback_t callback) {
    mission_ack_callback = callback;
}


void mavlink_parse_message(const mavlink_message_t *msg)
{
    // if (msg->sysid == 1 && msg->compid == 1) {
    //     return; // 跳过自己发送的消息
    // }
    
    switch (msg->msgid)
    {
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            mavlink_heartbeat_t hb;
            mavlink_msg_heartbeat_decode(msg, &hb);

            ESP_LOGI(TAG,
                "Heartbeat: type=%d, autopilot=%d, base_mode=%d, custom_mode=%" PRIu32 ", system_status=%d",
                hb.type, hb.autopilot, hb.base_mode, hb.custom_mode, hb.system_status);

            mavlink_state.custom_mode = hb.custom_mode;
            mavlink_state.prearm_status.system_standby = (hb.system_status == MAV_STATE_STANDBY);
            mavlink_state.prearm_status.not_armed = !(hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED);

            if (hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
                mavlink_state.is_armed = true;
                mavlink_state.arm_command_sent = false;
                ESP_LOGI(TAG, "已解锁");
            } else {
                mavlink_state.is_armed = false;
                ESP_LOGI(TAG, "未解锁");
            }

            // mavlink_state.self_check_passed = mavlink_check_prearm_ready();
            break;
        }

        case MAVLINK_MSG_ID_STATUSTEXT:
        {
            mavlink_statustext_t status_text;
            mavlink_msg_statustext_decode(msg, &status_text);

            // 添加详细调试日志
            ESP_LOGI(TAG, "收到STATUSTEXT: [%s]", status_text.text);
            ESP_LOGI(TAG, "当前no_prearm_errors状态: %s", 
                    mavlink_state.prearm_status.no_prearm_errors ? "true" : "false");

            mavlink_state.prearm_status.no_prearm_errors = true;
            if (strstr((char*)status_text.text, "PreArm") || 
                strstr((char*)status_text.text, "Failsafe")) {
                mavlink_state.prearm_status.no_prearm_errors = false;
                mavlink_state.prearm_status.last_prearm_error_time = xTaskGetTickCount() * portTICK_PERIOD_MS;
                ESP_LOGW(TAG, "检测到预解锁错误: %s", status_text.text);
            } else if (strstr((char*)status_text.text, "Ready to Arm") ||
                      strstr((char*)status_text.text, "Arming checks passed")) {
                mavlink_state.prearm_status.no_prearm_errors = true;
                mavlink_state.prearm_status.last_prearm_error_time = 0;
                ESP_LOGI(TAG, "预解锁检查通过: %s", status_text.text);
            }
            
            // mavlink_state.self_check_passed = mavlink_check_prearm_ready();
            break;
        }

        case MAVLINK_MSG_ID_UAV_MAVLINK_ID:  // 添加这个新的case
        {
            mavlink_uav_mavlink_id_t uav_id_msg;
            mavlink_msg_uav_mavlink_id_decode(msg, &uav_id_msg);
            
            ESP_LOGI(TAG, "收到飞控UAV_ID原始值: %d (0x%X)", 
                    uav_id_msg.uav_id, uav_id_msg.uav_id);

            // 根据飞控发送的ID更新设备ID
            uint32_t new_device_id = (uint32_t)uav_id_msg.uav_id;
            
            // 验证ID范围：0x00000000 ~ 0xFF00FFFF
            if (new_device_id <= 0xFF00FFFF) {
                g_device_id = new_device_id;
                ESP_LOGI(TAG, "设备ID已更新: 0x%" PRIx32, g_device_id);
                
                // 重新初始化签名功能（因为签名基于设备ID）
                // signature_sign_initialized = false;
                // if (init_signature_signing()) {
                //     ESP_LOGI(TAG, "签名功能已重新初始化");
                // } else {
                //     ESP_LOGW(TAG, "签名功能重新初始化失败");
                // }
            } else {
                ESP_LOGW(TAG, "收到的设备ID超出范围: 0x%" PRIx32 "，忽略", new_device_id);
            }
            break;
        }

        case MAVLINK_MSG_ID_EKF_STATUS_REPORT:
        {
            mavlink_ekf_status_report_t ekf_status;
            mavlink_msg_ekf_status_report_decode(msg, &ekf_status);
            
            mavlink_state.prearm_status.ekf_ok = (ekf_status.flags & EKF_ATTITUDE) != 0;
            
            // ESP_LOGI(TAG, "EKF状态: flags=0x%04x, 姿态OK=%s", 
            //          ekf_status.flags, 
            //          mavlink_state.prearm_status.ekf_ok ? "是" : "否");
            
            // mavlink_state.self_check_passed = mavlink_check_prearm_ready();
            break;
        }

        case MAVLINK_MSG_ID_GPS_RAW_INT:
        {
            mavlink_gps_raw_int_t gps;
            mavlink_msg_gps_raw_int_decode(msg, &gps);

            mavlink_state.prearm_status.gps_ok = (gps.fix_type >= 3) && 
                                                (gps.satellites_visible >= MIN_GPS_SATELLITES);
            
            // ESP_LOGI(TAG, "GPS状态: fix_type=%d, satellites=%d, GPS_OK=%s", 
            //          gps.fix_type, 
            //          gps.satellites_visible,
            //          mavlink_state.prearm_status.gps_ok ? "是" : "否");
            
            // mavlink_state.self_check_passed = mavlink_check_prearm_ready();
            break;
        }

        case MAVLINK_MSG_ID_GUIDE_ACK:
        {
            guide_ack_info_t ack_info;
            bool success = 0;
            
            if (mavlink_parse_guide_ack_message(msg, &ack_info) == 0) {
                ESP_LOGI(TAG, "  飞控返回结果: %s (标志位: %d)",
                        ack_info.result_msg, ack_info.upload_flag);
                
                if (xSemaphoreTake(task_tracker_mutex, pdMS_TO_TICKS(100)) == pdTRUE) {
                    ESP_LOGI(TAG, "当前任务模式: %d", task_tracker.task_code);
                    
                    // 检查是否已等待足够时间（200ms）
                    uint32_t current_time_ack = xTaskGetTickCount();
                    uint32_t elapsed_ms = (current_time_ack - task_tracker.timeout_timer) * portTICK_PERIOD_MS;
                    
                    if (elapsed_ms < 200) {
                        // ESP_LOGI(TAG, "GUIDE_ACK过早到达，已等待%dms，忽略", elapsed_ms);
                        xSemaphoreGive(task_tracker_mutex);
                        break; // 忽略这次ACK
                    }
                    
                    // 原有的处理逻辑
                    if (task_tracker.task_code == 0x0001) {
                        if (ack_info.result == GUIDE_ACK_SUCCESS) {
                            success = 1;
                        } else {
                            success = 0;
                        }
                    }
                    if (task_tracker.task_code == 0x0002) {
                        if (ack_info.result == GUIDE_ACK_FAILED) {
                            success = 1;
                        } else {
                            success = 0;
                        }
                    }
                    
                    xSemaphoreGive(task_tracker_mutex);
                } else {
                    ESP_LOGW(TAG, "获取task_tracker互斥锁超时");
                    success = 0;
                }
                
                // 通知主程序
                if (guide_ack_callback != NULL) {
                    guide_ack_callback(success);
                }
            }
            break;
        }

        // 处理航线相关的应答消息
        case MAVLINK_MSG_ID_MISSION_ACK:
        {
            mavlink_mission_ack_t ack;
            mavlink_msg_mission_ack_decode(msg, &ack);
            
            ESP_LOGI(TAG, "航线应答: 类型=%d, 目标系统=%d, 目标组件=%d, task_code=%d", 
                     ack.type, ack.target_system, ack.target_component, task_tracker.task_code);

            bool success = (ack.type == MAV_MISSION_ACCEPTED);
            
            if (success) {
                ESP_LOGI(TAG, "✅ 航线已被接受");
                // 更新航线状态为完成
                mavlink_set_mission_state(MISSION_STATE_FINISHED);
            } else {
                ESP_LOGI(TAG, "❌ 航线被拒绝，错误码: %d", ack.type);
                // 更新航线状态为空闲
                mavlink_set_mission_state(MISSION_STATE_IDLE);
            }
            
            // 通知主程序   
            if (mission_ack_callback != NULL) {
                mission_ack_callback(success);
            }
            
            break;
        }

        case MAVLINK_MSG_ID_MISSION_REQUEST:
        {
            mavlink_mission_request_int_t req;
            mavlink_msg_mission_request_int_decode(msg, &req);
            
            ESP_LOGI(TAG, "飞控请求航线点: seq=%d", req.seq);
            
            // 安全读取状态
            mission_state_t state = mavlink_get_mission_state();
            uint16_t count = mavlink_get_mission_count();
            
            // 验证请求的有效性
            if (state != MISSION_STATE_UPLOADING) {
                ESP_LOGW(TAG, "当前不在航线上传状态，忽略请求");
                break;
            }
            
            if (!has_valid_mission_data()) {
                ESP_LOGE(TAG, "没有有效的航线数据");
                break;
            }
            
            if (req.seq >= count) {
                ESP_LOGE(TAG, "请求的航点序号超出范围: %d >= %d", req.seq, count);
                break;
            }
            
            // 更新当前索引
            mavlink_set_mission_index(req.seq);
            
            // if (req.seq > 0) {
            //     req.seq = req.seq - 1;
            // }
            // 发送请求的航点（传递序号参数）
            send_mission_items(MAVLINK_UART_NUM, req.target_system, req.target_component, req.seq);
            
            // 检查是否完成所有航点发送
            if (req.seq == count - 1) {
                // mavlink_set_mission_state(MISSION_STATE_FINISHED);
                ESP_LOGI(TAG, "所有航线点发送完成，等待飞控确认...");
            }
            
            break;
        }
        case MAVLINK_MSG_ID_SYS_STATUS:
        {
            mavlink_sys_status_t req;
            mavlink_msg_sys_status_decode(msg, &req);

            // 检查 PREARM_CHECK 位
            bool prearmEnabled = req.onboard_control_sensors_enabled & MAV_SYS_STATUS_PREARM_CHECK;
            bool prearmHealthy = req.onboard_control_sensors_health & MAV_SYS_STATUS_PREARM_CHECK;
            bool readyFly = prearmEnabled && prearmHealthy;
            ESP_LOGI(TAG, "Result: _readyToFly = << %d;", readyFly);
            mavlink_set_self_check_status(readyFly);
            break;
        }

        default:
            break;
    }
}

void mavlink_handler_init(void)
{
    mavlink_uart_init();
    ESP_LOGI(TAG, "MAVLink Handler initialized");
}

void mavlink_task(void *pvParameters)
{
    ESP_LOGI(TAG, "MAVLink Task started");
    
    uint8_t rx_buf[256];
    size_t rx_len;
    uint32_t heartbeat_timer = 0;
    
    while (1)
    {
        // 接收串口数据
        rx_len = uart_read_bytes(MAVLINK_UART_NUM, rx_buf, sizeof(rx_buf), 10 / portTICK_PERIOD_MS);
        if (rx_len > 0)
        {   
            // 可选：打印前32个字节的十六进制数据用于调试
            // if (rx_len <= 32) {
            //     ESP_LOG_BUFFER_HEX(TAG, rx_buf, rx_len);
            // } else {
            //     ESP_LOG_BUFFER_HEX(TAG, rx_buf, 32);  // 只打印前32字节
            //     ESP_LOGI(TAG, "... (还有 %d 字节)", rx_len - 32);
            // }

            for (int i = 0; i < rx_len; i++)
            {             
                uint8_t res = mavlink_parse_char(MAVLINK_COMM_0, rx_buf[i], &rx_msg, &rx_status);
                
                if (res == MAVLINK_FRAMING_OK)
                {
                    mavlink_parse_message(&rx_msg);
                }
            }
        }

        // 每1秒发送一次 heartbeat
        if (++heartbeat_timer >= 100) {  // 100 * 10ms = 1000ms
            mavlink_send_heartbeat();
            heartbeat_timer = 0;
        }
        vTaskDelay(10 / portTICK_PERIOD_MS);
    }
}

// 发送起飞指令
void send_takeoff_command(float altitude) {
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
    
    mavlink_msg_command_long_pack(
        1, 1,                    // 系统ID, 组件ID
        &msg,
        target_system,           // 目标系统
        target_component,        // 目标组件
        MAV_CMD_NAV_TAKEOFF,     // 起飞命令
        0,                       // 确认
        0, 0, 0, 0,             // param1-4
        0, 0,                   // param5-6 (经纬度，通常为0)
        altitude                // param7: 目标高度
    );
    
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(MAVLINK_UART_NUM, (const char*)buf, len);
    ESP_LOGI(TAG, "发送起飞指令，目标高度: %.1fm", altitude);
}

// 切换到航线模式
void switch_to_mission_mode(void) {
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
    
    mavlink_msg_command_long_pack(
        1, 1,                    // 系统ID, 组件ID
        &msg,
        target_system,           // 目标系统
        target_component,        // 目标组件
        MAV_CMD_DO_SET_MODE,     // 设置模式命令
        0,                       // 确认
        MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,  // param1: 基础模式
        3,                       // param2: AUTO模式(航线模式)
        0, 0, 0, 0, 0           // param3-7
    );
    
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(MAVLINK_UART_NUM, (const char*)buf, len);
    ESP_LOGI(TAG, "切换到航线模式");
}

void switch_to_guide_mode(void) {
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
    
    mavlink_msg_command_long_pack(
        1, 1,                    // 系统ID, 组件ID
        &msg,
        target_system,           // 目标系统
        target_component,        // 目标组件
        MAV_CMD_DO_SET_MODE,     // 设置模式命令
        0,                       // 确认
        MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,  // param1: 基础模式
        4,                       // param2: AUTO模式(航线模式)
        0, 0, 0, 0, 0           // param3-7
    );
    
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(MAVLINK_UART_NUM, (const char*)buf, len);
    ESP_LOGI(TAG, "切换到GUIDE模式");
}

void switch_to_throw_mode(void) {
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
    
    mavlink_msg_command_long_pack(
        1, 1,                    // 系统ID, 组件ID
        &msg,
        target_system,           // 目标系统
        target_component,        // 目标组件
        MAV_CMD_DO_SET_MODE,     // 设置模式命令
        0,                       // 确认
        MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,  // param1: 基础模式
        18,                       // param2: 
        0, 0, 0, 0, 0           // param3-7
    );
    
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    uart_write_bytes(MAVLINK_UART_NUM, (const char*)buf, len);
    ESP_LOGI(TAG, "切换到GUIDE模式");
}

void mavlink_send_guide_target(int32_t lat, int32_t lon, float alt, int8_t target_type)
{
    mavlink_send_guide_board_target(255, 1, lat, lon, alt, target_type);
}

void mavlink_force_arm(bool arm)
{
    mavlink_send_arm_disarm_command(255, 1, arm);
}

void mavlink_mission_clear_all()
{
    send_mission_clear_all(MAVLINK_UART_NUM, 255, 1);
}